/*
* IMU_Dien.c
*
* Created: 4/21/2013 8:56:58 AM
*  Author: Ha Cac
*/

//#define _3DOF_
#define _5DOF_
//#define _6DOF_
//#define _9DOF_


#include <avr/io.h>
#include <avr/interrupt.h>

//#include <avr/pgmspace.h>
//#include <avr/eeprom.h>

#include <util/delay.h>

//#include <inttypes.h>
//#include <math.h>

//#include <stdio.h>
//#include <stdlib.h>
//#include "string.h"

#include "MyConfig.h"
#include "MyIMU.h"

// USART Receiver interrupt service routine
ISR(USART_RX_vect)
{
	char status,data;
	status = UCSR0A;
	data = UDR0;
	if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN)) == 0)
	{
		rx_buffer0[rx_wr_index0++] = data;
		#if RX_BUFFER_SIZE0 == 256
		// special case for receiver buffer size=256
		if (++rx_counter0 == 0)
		{
			#else
			if (rx_wr_index0 == RX_BUFFER_SIZE0) rx_wr_index0 = 0;
			if (++rx_counter0 == RX_BUFFER_SIZE0)
			{
				rx_counter0 = 0;
				#endif
				//				rx_buffer_overflow0 = 1;
			}
		}
	}

	// dung duy nhat timer0 cho chuong trinh quad
	volatile unsigned long timer0_millis = 0;

	SIGNAL(TIMER0_OVF_vect)
	{
		// copy these to local variables so they can be stored in registers
		// (volatile variables must be read from memory on every access)
		unsigned long m = timer0_millis;
		m += 1;
		timer0_millis = m;
		TCNT0 = TCNT0_SET_1000MICROSEC;	// 1000 microsec tran 1 lan
	}

	typedef struct {
		double q; //process noise covariance
		double r; //measurement noise covariance
		double x; //value
		double p; //estimation error covariance
		double k; //kalman gain
	} kalman_state;

	kalman_state kalman_init(double p, double q, double r, double intial_value)
	{
		kalman_state result;
		result.q = q;
		result.r = r;
		result.p = p;
		result.k = 0.0;
		result.x = intial_value;

		return result;
	}

	kalman_state mykm;

	double kalman_update(kalman_state* state, double measurement)
	{
		//prediction update
		//omit x = x
		state->p = state->p + state->q;

		//measurement update
		state->k = state->p / (state->p + state->r);
		state->x = state->x + state->k * (measurement - state->x);
		state->p = (1 - state->k) * state->p;
		return state->x;
	}
	/*-------------------------------------------------------*/
	// Main functions
	/*-------------------------------------------------------*/

	static unsigned char led_timer = 0;
	unsigned char setup_status;				// kiem tra trang thai setup
	kalman_state mykm;
	float BaroKalman;
	//float Omega[3];
	float val[9];
	float q[4];
	unsigned long now, last;

	int main(void)
	{
		asm volatile("cli");
		MyMCU_Init();
		MyMCU_TestSetup();
		MyUSART_Init();
		MyI2C_Init();
		i2c_init();
		MyOutput_Init();
		MyInput_Init();
		MyIMU_Init();
		
		//	BMA180_Init(BMA180_ADDRESS_SDO_LOW);
		//	ITG3200_Init(ITG3200_ADDR_AD0_HIGH);
		//	HMC5883_Init(HMC5883L_ADDR, 0);

		MyTimer0_Init();
		asm volatile("sei");

		mykm = kalman_init(0.05, 0.125, 32, 0.0);

		// khoi dong dem tan so
		last = millis();
		// main loop
		while (1)
		{
			//		Baro_update();
			//		BaroKalman = kalman_update(&mykm, BaroAlt);
			//		SerialFloatPrint(BaroAlt);
			//		SerialPrint(',');
			//		SerialFloatPrint(BaroKalman);
			//		SerialPrint(',');
			
			//MyIMU_getQuaternion(q);
			//SerialPrintFloatArr(q, 4);

			// thoi gian 50ms (20Hz) -> co the thay halfT = 25; trong AHRS proc
			now = millis();
			while (now - last < 50) now = millis();
			last = now;

			//		MPU6050_getMotion9(&val[0], &val[1], &val[2], &val[3], &val[4], &val[5], &val[6], &val[7], &val[8]);
			//		SerialPrintFloatArr(&val[0], 9);
			//		BMA180_getValues(&val[0], &val[1], &val[2]);
			//		ITG3200_getValues(&val[3], &val[4], &val[5]);
			//		HMC5883_getValues(&val[6], &val[7], &val[8]);
			//		SerialPrintFloatArr(&val[6], 3);

			led_timer += 1;
			if (led_timer == 20) led_timer = 0;
			if (led_timer < 10)
			cbi(PORT_LED, LED);
			else sbi(PORT_LED, LED);
		}
		return 0;
	}
